from moveit_configs_utils import MoveItConfigsBuilder
from moveit_configs_utils.launches import generate_demo_launch


from launch import LaunchDescription
from launch_ros.actions import Node



def generate_launch_description():
    moveit_config = MoveItConfigsBuilder("test_urdf", package_name="zc4_moveit_config").to_moveit_configs()
    

    # Node for the simple_planning_demo
    simple_planning_demo_node = Node(
        package='simple_planning_demo',
        executable='simple_planning_demo',
        name='simple_planning_demo',
        output='screen',
        parameters=[
            moveit_config.robot_description,  # Load URDF
            moveit_config.robot_description_semantic,  # Load SRDF
            moveit_config.robot_description_kinematics,  # Load kinematics.yaml
        ],        
    )
 
    # Create the launch description and populate it
    ld = LaunchDescription()
 
    # Add the simple_planning_demo node to the launch description
    ld.add_action(simple_planning_demo_node)


    return generate_demo_launch(moveit_config)
